﻿#include "stdafx.h"
#include "robot.h"
#include "RobotClient.h"

Robot::Robot(bool isContinousAcq, CObject *parent) 
 //   CObject(parent)
{
    //定义并且启动定时获取机器人位姿的定时器
 //   tmr = new QTimer(this);
 //   connect(tmr,SIGNAL(timeout()),this,SLOT(onTimer()));
 //   if(isContinousAcq)
 //   tmr->start(300);
}    

Robot::~Robot()
{
}

void Robot::onTimer()
{
    currPose = getPose();
 //   emit currPoseReady(currPose);        //显示机器爪当前位置
}

//控制机器人进行运动
int Robot::setPose(Move m)
{
    float xyzwpr[9] = {m.p.x, m.p.y, m.p.z, m.p.rx, m.p.ry, m.p.rz, 0,0,0};
    short speed = m.speed;

    char recvCmd[256] = "";
  //  emit isRobotMoving(true);
  //   emit log(tr("Moving robot..."));
    sendCmd(ControlRobot_SetSpeed(speed), recvCmd);
    sendCmd(ControlRobot_SetXyzwpr(xyzwpr), recvCmd);
 //    emit isRobotMoving(false);
 //   emit log(tr("Robot stops."));
    return 1;
}

//通知机器人进行运动
void Robot::onMoveRobot(Move m)
{
	Robot R;
	R.setPose(m);
}

Pose Robot::getPose()
{

    char recvCmd[256];
    sendCmd(ControlRobot_GetCoord(), recvCmd);
    float xyzwpr[9] = {0,0,0,0,0,0,0,0,0};
    Cmd2Coord(recvCmd, xyzwpr, nullptr, nullptr);

    for(int i=0; i<9; i++)
    {
        std::cout<<"XYZWPR "<<i<<":  "<<xyzwpr[i]<<std::endl;
    }

    Pose p = genPose(xyzwpr[0],xyzwpr[1],xyzwpr[2],xyzwpr[3],xyzwpr[4],xyzwpr[5]);
    return p;
}

int Robot::switchHand(int s)
{
    char msg[100];
    if(s==1)
        sendCmd(ControlHand(HAND_CLOSED),msg);
    else if(s==0)
        sendCmd(ControlHand(HAND_OPENED),msg);
    return 1;
}

int Robot::switchGripper(int s)
{
    char msg[100];
    if(s==1)
        sendCmd(ControlGripper(GRIPPER_CLOSED),msg);
    else if(s==0)
        sendCmd(ControlGripper(GRIPPER_OPENED),msg);
    return 1;
}



int Robot::switchVavle2(int s)
{
  //  emit log(tr("control vavle2..."));
    char msg[100];
    if(s==0)
        sendCmd(ControlVavle(VAVLE_CLOSED),msg);
    else if(s==1)
        sendCmd(ControlVavle(VAVLE_OPENED),msg);
    return 1;
}

int Robot::switchPlc(int s)
{
  //  emit log(tr("control plc..."));
    char msg[100];
    if(s==0)
        sendCmd(ControlPLC(PLC_CLOSED),msg);
    else if(s==1)
        sendCmd(ControlPLC(PLC_OPENED),msg);
    return 1;
}

int Robot::switchMotor(int s)
{
 //   emit log(tr("control motor..."));
    char msg[100];
    if(s==1)
        sendCmd(ControlStepMotor_Move(MOTOR_FORWARD, 20, 5),msg);
    else if(s==0)
        sendCmd(ControlStepMotor_Move(MOTOR_BACKWARD, 20, 5),msg);
    return 1;
}

int Robot::switchcylinder1(int s)
{
	char msg[100];
	if (s == 1)
		sendCmd(Controlcylinder1(ON), msg);
	else if (s == 0)
		sendCmd(Controlcylinder1(OFF), msg);
	return 1;
}
int Robot::switchcylinder2(int s)
{
	char msg[100];
	if (s == 1)
		sendCmd(Controlcylinder2(ON), msg);
	else if (s == 0)
		sendCmd(Controlcylinder2(OFF), msg);
	return 1;
}
int Robot::switchcylinder3(int s)
{
	char msg[100];
	if (s == 1)
		sendCmd(Controlcylinder3(ON), msg);
	else if (s == 0)
		sendCmd(Controlcylinder3(OFF), msg);
	return 1;
}
int Robot::switchcylinder4(int s)
{
	char msg[100];
	if (s == 1)
		sendCmd(Controlcylinder4(ON), msg);
	else if (s == 0)
		sendCmd(Controlcylinder4(OFF), msg);
	return 1;
}
int Robot::switchcylinder5(int s)
{
	char msg[100];
	if (s == 1)
		sendCmd(Controlcylinder5(ON), msg);
	else if (s == 0)
		sendCmd(Controlcylinder5(OFF), msg);
	return 1;
}
int Robot::switchcylinder6(int s)
{
	char msg[100];
	if (s == 1)
		sendCmd(Controlcylinder6(ON), msg);
	else if (s == 0)
		sendCmd(Controlcylinder6(OFF), msg);
	return 1;
}

void Robot::onswitchcylinder1(int s)
{
	switchcylinder1(s);
}
void Robot::onswitchcylinder2(int s)
{
	switchcylinder2(s);
}
void Robot::onswitchcylinder3(int s)
{
	switchcylinder3(s);
}
void Robot::onswitchcylinder4(int s)
{
	switchcylinder4(s);
}
void Robot::onswitchcylinder5(int s)
{
	switchcylinder5(s);
}
void Robot::onswitchcylinder6(int s)
{
	switchcylinder6(s);
}
//通知机器人切换手抓状态
void Robot::onSwitchHand(int s)
{
    switchHand(s);
}
//通知机器人切换滑台夹具状态
void Robot::onSwitchGripper(int s)
{
    switchGripper(s);
}

//通知机器人切换PLC开关状态
void Robot::onSwitchPlc(int s)
{
    switchPlc(s);
}
//通知机器人切换PLC开关状态
void Robot::onSwitchMotor(int s)
{
    switchMotor(s);
}

//通知机器人切换连杆夹具状态
void Robot::onSwitchVavle2(int s)
{
    switchVavle2(s);
}


